#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iostream>

using namespace Eigen;
using namespace std;


Quaterniond euler2Quaternion( const Vector3d& euler)
{
    Eigen::AngleAxisd rollAngle(euler[0], Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(euler[1], Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(euler[2], Eigen::Vector3d::UnitZ());

    Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;

    return q;
}

Translation3d xyz2Tanslation(const Vector3d& xyz) {
    return Translation3d(xyz[0],xyz[1],xyz[2]);
}

Matrix4d getTransform(const Vector3d& euler,const Vector3d& xyz) {
    return (xyz2Tanslation(xyz)* euler2Quaternion(euler)).matrix();
}

void transform2Euler(const Matrix4d& transform, Vector3d& euler) {
    Matrix3d rotation= transform.block<3,3>(0,0);

    euler= rotation.eulerAngles(0,1,2);
}

void transform2Xyz(const Matrix4d& transform, Vector3d& xyz)
{
    xyz =transform.block<3,1>(0,3);

}

int main(int argc, char *argv[])
{
    Vector3d object2CameraT, object2CameraR,object2WorldT,object2WorldR;


    object2CameraT<<0.530864,0.154085,0.0347962;
    object2CameraR<<0.0697259,0.798108,0.0156702;

    object2WorldT<<0,0,0;
    object2WorldR<<0,0,0;

    Matrix4d object2CameraM = getTransform(object2CameraR,object2CameraT);
    Matrix4d object2WorldM = getTransform(object2WorldR,object2WorldT);
    Matrix4d camera2ObjectM = object2CameraM.inverse();
    Matrix4d camera2WorldM = camera2ObjectM*object2WorldM;

    cout<<"tranform is"<<endl<<camera2WorldM<<endl;


    Vector3d camera2WorldT,camera2WorldR;
    transform2Xyz(camera2WorldM,camera2WorldT);
    transform2Euler(camera2WorldM,camera2WorldR);

    cout<<"x,y,z is:"<<endl<<camera2WorldT<<endl;
    cout<<"roll, pitch, yaw is:"<<endl<<camera2WorldR<<endl;

    Matrix3d rotation;
    rotation<<-0.018085113,0.14604087,0.98907423,
            -0.99984187,-0.0080606751,-0.017166384,
            0.0054173553,-0.98926067,0.14607099;
    Quaterniond q = Quaterniond(rotation).normalized();

    cout<<q.x()<<","<<q.y()<<","<<q.z()<<","<<q.w()<<endl;


    return 0;
}

